﻿#include <algorithm>
#include <gtest/gtest.h>
#include <iostream>
#include <vector>

#include <learn/alg/alg_approximator.h>
#include <learn/utils/output.h>

using namespace xi;
using namespace xi::alg;

TEST(TestApproximator, iterator)
{
    Alg_LaneRiesenfeld<3> interpolator;

    interpolator.cpts.push_back(Vec3{0.0, 0.0, 0.0});
    interpolator.cpts.push_back(Vec3{0.0, 1.0, 0.0});
    interpolator.cpts.push_back(Vec3{1.0, 1.0, 0.0});
    interpolator.cpts.push_back(Vec3{1.0, 0.0, 0.0});

    auto func = interpolator.generate();
    for (auto x : func(1))
        std::cout << x << " ";
    std::cout << std::endl;
}

TEST(TestApproximator, Energy)
{
    {
        Alg_BSplineCurveEnergy_Approximator<3> curveApprox;

        curveApprox.wBending = 1e-1;
        curveApprox.wStretch = 1e-8;

        auto f = [](double x) -> Vec3 { return Vec3{x, x * x, x * x * x}; };
        for (int i = 0; i < 100; i++)
        {
            curveApprox.data.push_back(f(i / 100.0));
        }

        auto curve = curveApprox.approximate(10);
        auto poles = curve->poles();
        Eigen::MatrixXd A(poles.size(), 3);
        for (int i = 0; i < poles.size(); i++)
        {
            A.row(i) << poles[i].x(), poles[i].y(), poles[i].z();
        }

        auto B0 = Alg_CoeffMatrix::curve_least_square(curveApprox.params, curve->knots(), curveApprox.degree);
        auto [B1, B2] = Alg_CoeffMatrix::curve_stretch_bending(curve->knots(), curveApprox.degree);
        auto Fitting = A.transpose() * B0.transpose() * B0 * A;
        auto Stretching = A.transpose() * B1 * A;
        auto Bending = A.transpose() * B2 * A;
        std::cout << Fitting.trace() << std::endl;
        std::cout << Stretching.trace() << std::endl;
        std::cout << Bending.trace() << std::endl;
    }

    {
        Alg_BSplineSurfaceEnergy_Approximator<3> surfaceApprox;

        surfaceApprox.wBending = 1e-8;
        surfaceApprox.wStretch = 1e-1;
        surfaceApprox.uDegree = 3;
        surfaceApprox.vDegree = 2;

        auto f = [](double u, double v) -> Vec3 { return Vec3{u, v, sin(u + v) * cos(u - v)}; };

        // 20 x 30 个采样点
        int N = 20;
        int M = 30;

        std::vector<Vec3> points(N * M);
        for (int i = 0; i < N; i++)
            for (int j = 0; j < M; j++)
                surfaceApprox.data.push_back(f(5.0 / (N - 1) * i, 5.0 / (M - 1) * j));

        auto surface = surfaceApprox.approximate(10, 8);

        auto poles = surface->poles();
        Eigen::MatrixXd A(poles.size(), 3);
        for (int i = 0; i < poles.size(); i++)
        {
            A.row(i) << poles[i].x(), poles[i].y(), poles[i].z();
        }

        auto B0 = Alg_CoeffMatrix::surface_least_square(surfaceApprox.params, surface->u_knots(), surface->v_knots(),
                                                        surfaceApprox.uDegree, surfaceApprox.vDegree);
        auto [B1, B2] = Alg_CoeffMatrix::surface_stretch_bending(surface->u_knots(), surface->v_knots(),
                                                                 surfaceApprox.uDegree, surfaceApprox.vDegree);
        auto Fitting = A.transpose() * B0.transpose() * B0 * A;
        auto Stretching = A.transpose() * B1 * A;
        auto Bending = A.transpose() * B2 * A;
        std::cout << Fitting.trace() << std::endl;
        std::cout << Stretching.trace() << std::endl;
        std::cout << Bending.trace() << std::endl;
    }
}

TEST(TestApproximator, NURBS)
{
    // NURBS 两步线性拟合算法
    {
        Alg_NURBSCurve_Approximator<3> approx;

        auto f = [](double x) -> Vec3 { return Vec3{x, x * x, x * x * x}; };
        for (int i = 0; i < 100; i++)
            approx.data.push_back(f(i / 100.0));

        auto curve = approx.approximate(10);
    }
}